//control.c

#include<stdio.h>

int kp=5;

//unsigned long McBSP0_slave(unsigned long Value);

void wheel_control(int left, int right)
{
	int left_dir=0;
	int right_dir=0;
	unsigned int left_data=0;
	unsigned int right_data=0;			

	if(left > 0)
		left_dir = 1;
	else
		left_dir = 0;

	if(right > 0)
		right_dir = 1;
	else
		right_dir = 0;

	if(left < 0)
		left = -left;

	if(right < 0)
		right = -right;

    left=1000+(int)(((double)1000)/100*left);
	right=1000+(int)(((double)1000)/100*right);

    if(left > 2000)
    {
		left = 1;
		left = 2;
//		printf("wrong!\n");
	}
	else{
//		printf("@#$&@#$^!\n");
		}


	left_data= ((0xA)<<12)|(left << 1) |(left_dir);
    right_data= ((0xB)<<12)|(right << 1) |(right_dir);
	//left_data=  0x10FA;
    //right_data=0x01AF;
//	McBSP0_slave((unsigned long)0xAFF0);
//	McBSP0_slave((unsigned long)0xBFFF);
	McBSP0_slave((unsigned long)left_data);
	McBSP0_slave((unsigned long)right_data);



}


void control(int xpos,int ypos,int mode,int ref)
{
	int turn=0;
	int left_wheel=0;
	int right_wheel=0;

	//printf("mode %d\n", mode);


	if (mode==0)
	{//tracking
		turn=-kp*(xpos-178);
		//printf("turn %d\n", turn);
		//printf("ref %d\n", ref);
		if (turn>500)
		{
			turn=500;
		}
		if (turn<-500)
		{
			turn=-500;
		}	
		if(turn>=0)
		{
			left_wheel=ref;
			right_wheel=ref*((1000-turn)/1000);
			//printf("left_wheel %d\n", left_wheel);
			//printf("right_wheel %d\n", right_wheel);
		}
		else
		{
			left_wheel=ref*(1000+turn)/1000;
			right_wheel=ref;
		}	
	}

	if (mode==1)
	{//left pivot
			left_wheel=0;
			right_wheel=ref;
	}
	//mode=49;
	
	//mode=0;
	//printf("mode %X\n", mode);
	//printf("mode %X\n", mode);
	if (mode==2)
	{  //right pivot
		//printf("mode %d\n", mode);
		left_wheel=ref;
		right_wheel=0;
	}

	left_wheel = left_wheel/10;
	right_wheel = right_wheel/10;





	wheel_control( left_wheel , right_wheel );

	
}

